#include "motor_b_foc_config.h"
// FOC框架
#include "foc.h"
// 电机B的电机使能
#include "motor_b_enable.h"
// 电机B的SVPWM实现
#include "motor_b_svpwm_config.h"
// 电机B的ADC电流检测
#include "motor_b_adc2_current.h"
// 电机B的传感器实现
#include "motor_b_encoder_config.h"


// PID参数
#define MB_OUTMAX		12.0f * 0.577f		// 供电电压12V
#define MB_KP			0.018f
#define MB_KI			0.018f
#define MB_KD			0.0f				// 不使用微分控制

// 电流检测参数
#define AD_TO_CURRENT	0.00806				// 电流检测 	0.00032


void MOTORB_PreCurrent(float *ua, float *ub, float *uc);

// 定义电机B的FOC结构体
FOC_Struct MotorB_FOC = 
{
	.isEnable = 0,		// 电机使能标志
	.polePairs = 7.0f,	// 极对数
	.tariq = 0.0f,	// 目标电流q
	.tarid = 0.0f,	// 目标电流d
	
	.mAngle		= 0.0f,	// 机械角度
	.eAngle		= 0.0f,	// 电角度
	.eRadian	= 0.0f,	// 电角度的弧度

	.iNum		= 2,		// 电流采样个数
	.ia			= 0.0f,		// a 相实际电流
	.ib			= 0.0f,		// b 相实际电流
	.ic			= 0.0f,		// c 相实际电流
    .i_alpha	= 0.0f,		// clarke变换后 * 2/3后 Iα
    .i_beta		= 0.0f,		// clarke变换后 * 2/3后 Iβ
    .iq			= 0.0f,		// park变换后 Iq
    .id			= 0.0f,		// park变换后 Id

    .i_alpha_SVPWM	= 0.0f,	// park反变换后 Iα
    .i_beta_SVPWM	= 0.0f,	// park反变换后 Iβ

	.idPID	= {0},		// d向量的pid参数结构体
	.iqPID	= {0},		// q向量的pid参数结构体
	
    .SetEnable			= MOTORB_SetEnable,
    .GetEncoderAngle	= MOTORB_GetEncoderAngle,
    .GetSVPWMSector		= MOTORB_GetSVPWMSector,
    .GetPreCurrent		= MOTORB_PreCurrent,
    .SvpwmGenerate		= MOTORB_SvpwmGenerate,
};

/**
 * @brief   更新电机B三相电流
 *
 * @param   none
 *
 * @return  none
 */
void MOTORB_PreCurrent(float *ua, float *ub, float *uc)
{
	*ua = MOTORB_GetADC2PhaseXValue(0) * AD_TO_CURRENT * 1.5;
	*ub = MOTORB_GetADC2PhaseXValue(1) * AD_TO_CURRENT * 1.5;
	*uc = MOTORB_GetADC2PhaseXValue(2) * AD_TO_CURRENT * 1.5;
}

/**
 * @brief   MOTORB FOC参数初始化
 *
 * @param   none
 *
 * @return  none
 */
void MOTORB_FOC_Config_Init(void)
{
	// 1. 使能电机B
	SetFocEnable(&MotorB_FOC, 0);
	
	// 2. 设置PID参数
	SetCurrentPIDParams(&MotorB_FOC, MB_KP, MB_KI, MB_KD, MB_OUTMAX);
	
	// 3. 设置目标电流
	SetCurrentPIDTar(&MotorB_FOC, 0, 0);
}

/**
 * @brief   电机B调试信息输出
 *
 * @param   none
 *
 * @return  none
 */
void MOTORB_FOC_Config_Printf(void)
{
	FocPrintf(&MotorB_FOC);
}

/**
 * @brief   电机B的FOC/电流/力矩环控制
 *
 * @param   none
 *
 * @return  none
 */
void MOTORB_FocControl(void)
{
	FocContorl(&MotorB_FOC);
}

/**
 * @brief   设置电机B的FOC/电流/力矩环控制目标
 *
 * @param   none
 *
 * @return  none
 */
void MOTORB_SetTarIDIQ(float id, float iq)
{
	SetCurrentPIDTar(&MotorB_FOC, id, iq);
}

/**
 * @brief   获取电机B机械角度
 *
 * @param   none
 *
 * @return  none
 */
float MOTORB_GetAngle(void)
{
	return GetFocAngle(&MotorB_FOC);
}

